﻿using System;
using System.Collections.Generic;
using System.Drawing;
using System.Threading;
using System.Diagnostics;
using Emgu.CV;
using Emgu.CV.Structure;
using Emgu.CV.Features2D;
using iMapa.Utils;
using iMapa.Backend.Calibration;


namespace iMapa.Backend
{
    public class AutoCalibration:Worker
    {
        private MessageQueue requestQueue;
        private MessageQueue responseQueue;

        public AutoCalibration(MessageQueue requestQueue, MessageQueue responseQueue)
        {
            this.requestQueue = requestQueue;
            this.responseQueue = responseQueue;
            
        }

        public SURFFeature[] ExtractFeatures(Image<Bgr,Byte> f)
        {
            SURFFeature[] features = null;
            if (f != null)
            {
                Image<Gray, Byte> frame = f.Convert<Gray, Byte>();
                SURFDetector detector = ConfigurationConstants.SURFPARAMS;
                try
                {
                    features = frame.ExtractSURF(ref detector);
                    frame.Dispose();
                    return features;
                }
                catch (Exception e)
                {
                    Trace.TraceError("Ex",e);
                }
            }
            return null;
        }


        public PointF[] MatchFeatures(SURFFeature[] modelFeatures, SURFFeature[] observedFeatures, Size modelImageSize)
        {
            
            if (observedFeatures != null && modelFeatures!=null && modelImageSize!=null && modelFeatures.Length>0 && observedFeatures.Length>0)
            {
                try
                {
                    SURFTracker tracker = new SURFTracker(modelFeatures);

                    HomographyMatrix homography;
                    homography = tracker.Detect(observedFeatures, ConfigurationConstants.SURF_UNIQUENESS_THRESHOLD);

                    if (homography != null && homography.IsValid(7.0))
                    {
                        PointF[] pts = ImgUtils.getCornerPoints(modelImageSize);
                        homography.ProjectPoints(pts);
                        return pts;
                    }
                }
                catch (Exception e)
                {
                    return null;
                } 
            }
            return null;
        }
        
        protected override void Process()
        {

            try
            {
                object o = requestQueue.Consume();
                if (o is CalibrationRequest)
                {
                    Stopwatch watch = Stopwatch.StartNew();
                    CalibrationRequest request = (CalibrationRequest)o;
                    CalibrationResponse response = new CalibrationResponse();
                    List<PointF[]> responseResults = new List<PointF[]>();

                    SURFFeature[] observedFeatures = ExtractFeatures(request.Frame);
                    foreach (MapCalibrateRequest r in request.MapRequests)
                    {
                        PointF[] modelReflectionPoints = MatchFeatures(r.ModelFeatures, observedFeatures, new Size(r.ModelImageWidth, r.ModelImageHeight));
                        responseResults.Add(modelReflectionPoints);
                    }
                    response.ModelOnObservedImage = responseResults.ToArray();
                    watch.Stop();
                    response.Duration = watch.ElapsedMilliseconds;
                    responseQueue.Produce(response);
                    request.Frame.Dispose();
                }
                else
                {
                    responseQueue.Produce(new object());
                }
            }
            catch (Exception e)
            {
                string messageBoxText = "Error occured:\n" +e.StackTrace;
                string caption = "Unexpected Exception";
                System.Windows.MessageBoxButton button = System.Windows.MessageBoxButton.OK;
                System.Windows.MessageBoxImage icon = System.Windows.MessageBoxImage.Warning;
                System.Windows.MessageBox.Show(messageBoxText, caption, button, icon);

            }
        }
        
        
        protected new void SetStopped()
        {
            base.SetStopped();
            responseQueue.Produce(new object());
        }
    }
}
    

